#include <gl/glut.h>
#include "GLUtil.h"
#include <vec.h>
#include "ConvertUtil.h"
#include "RigidBody.h"
#include "PhyEngine.h"
#include <math.h>


using namespace std;

#pragma comment(lib, "imager")


static GLint    ImageWidth=800;
static GLint    ImageHeight=800;

RigidBodyList* bodies;
PhyEngine* engine;

double timestep = 0.01;

void ArenderMesh()
{

	engine->update(timestep);

	glPolygonMode(GL_FRONT_AND_BACK, GL_LINE);

	for (int k=0; k<bodies->size(); k++)
	{
		RigidBody* body = bodies->at(k);
		Polygon* poly = bodies->at(k)->poly;

		glBegin(GL_LINES);

		for (int i=0; i<poly->edgeCount; i++)
		{
			Edge ee = poly->edgeList[i];

			Vert va = poly->vertList[ee.a];
			Vert vb = poly->vertList[ee.b];

			double cx = (va.x + vb.x) / 2.0;
			double cy = (va.y + vb.y) / 2.0;

			double nx = cx + ee.nx * 0.1;
			double ny = cy + ee.ny * 0.1;

			Vec2d vva = body->getPointBodyToWorld(Vec2d(va.x, va.y));
			Vec2d vvb = body->getPointBodyToWorld(Vec2d(vb.x, vb.y));

			Vec2d vvc = (vva + vvb) / 2.0;

			Vec2d vvn = body->getVectorBodyToWorld(Vec2d(ee.nx, ee.ny));

			Vec2d vvd = vvc + 0.1 * vvn;

			glVertex3d(vva[0], vva[1], 0);
			glVertex3d(vvb[0], vvb[1], 0);

			glVertex3d(vvc[0], vvc[1], 0);
			glVertex3d(vvd[0], vvd[1], 0);
		}

		for (int i=0; i<poly->vertCount; i++)
		{
			Vert vv = poly->vertList[i];

			double nx = vv.x + vv.nx * 0.1;
			double ny = vv.y + vv.ny * 0.1;

			Vec2d vvn = body->getVectorBodyToWorld(Vec2d(vv.nx, vv.ny));
			Vec2d vva = body->getPointBodyToWorld(Vec2d(vv.x, vv.y));

			Vec2d vvb = vva + 0.1 * vvn;

			glVertex3d(vva[0], vva[1], 0);
			glVertex3d(vvb[0], vvb[1], 0);
		}

		glEnd();
	}

}

void simInitialize()
{
	RigidBody* box1 = new RigidBody("recL.2obj", 10000, 0.1);
	RigidBody* box2 = new RigidBody("mine.2obj", 1, 0.1);

	box1->pos = Vec2d(0, -14);
	box2->pos = Vec2d(0, 4);

	box2->vel = Vec2d(0, -3);

	//box2->orientation = 3.1415926;

	bodies = new RigidBodyList();

	bodies->push_back(box1);
	bodies->push_back(box2);

	engine = new PhyEngine(bodies, true);

	//body->orientation = -3.1415926 / 4.0;

	//body->rotation = 0.1;

	//Vec2d vv = body->getWorldVelocity(Vec2d(0, 1));

	//printf("%f %f\n", vv[0], vv[1]);
	//printf("%f\n", body->moment);
}

void main()
{
	simInitialize();	

	//engine->update(timestep);

	glutInitDisplayMode(GLUT_DOUBLE | GLUT_RGBA);
	glutInitWindowPosition(100, 100);
	glutInitWindowSize(ImageWidth, ImageHeight);
	glutCreateWindow("FLIP");
	glutDisplayFunc(display);
	glutReshapeFunc(reshape);
	glutMouseFunc(mouse);
	glutMotionFunc(motion);
	glutKeyboardFunc(keyboard);

	glutMainLoop();
}